#include "camera_abstractor.h"

CameraAbstractor::CameraAbstractor() {
  camera = NULL;
  stream_grabber = NULL;
  pbuffer = NULL;
  hbuffer = NULL;
  initialized = false;
}

void CameraAbstractor::Initialize() {
  error_message = "";

  if (!initialized) {
    try {
      // Pega a instância de Transport Layer Factory
      CTlFactory& tlfactory = CTlFactory::GetInstance();

      // Cria um Transport Layer para câmeras de interface Gigabit Ethernet
      ITransportLayer *ptl = tlfactory.CreateTl(Pylon::CBaslerGigECamera::DeviceClass());

      // Se não criou (sabe-se lá porque), termina
      if (! ptl) {
        error_message = "Failed to create transport layer!";
        return;
      }

      // Pega a lista de todas as câmeras do tipo conectadas à interface
      DeviceInfoList_t devices;
      if (ptl->EnumerateDevices(devices) == 0) {
        error_message = "No camera present!";
        return;
      }

      // Instancia o objeto câmera, usado para acessar e configurar parâmetros da câmera real
      camera = new Pylon::CBaslerGigECamera(ptl->CreateDevice(devices[0]));

      // Abre (estabelece conexão) com a câmera
      camera->Open();

      // Pega o primeiro Grabber da câmera
      stream_grabber = new Pylon::CBaslerGigECamera::StreamGrabber_t(camera->GetStreamGrabber(0));

      // Open the stream grabber
      stream_grabber->Open();

      // Ajusta os parâmetros de aquisição
      camera->PixelFormat.SetValue(PixelFormat_Mono8);
      camera->Width.SetValue(900); // TODO: Ver o mínimo!
      camera->Height.SetValue(900); // TODO: Aqui também!
      camera->OffsetX.SetValue(196);
      camera->OffsetY.SetValue(32);

      image_width = camera->Width.GetValue(false);
      image_height = camera->Height.GetValue(false);

      //Desabilita o trigger de aquisição (não sei o q eh, nem porque essa operação está em escopo fechado)
      {
        GenApi::IEnumEntry* acquisitionStart = camera->TriggerSelector.GetEntry(TriggerSelector_AcquisitionStart);
        if (acquisitionStart && GenApi::IsAvailable(acquisitionStart)) {
          camera->TriggerSelector.SetValue(TriggerSelector_AcquisitionStart);
          camera->TriggerMode.SetValue(TriggerMode_Off);
        }
      }

      //Desabilita o trigger de frame (de novo, não sei o q eh, nem porque essa operação está em escopo fechado)
      {
        GenApi::IEnumEntry* frameStart = camera->TriggerSelector.GetEntry(TriggerSelector_FrameStart);
        if (frameStart && GenApi::IsAvailable(frameStart)) {
          camera->TriggerSelector.SetValue(TriggerSelector_FrameStart);
          camera->TriggerMode.SetValue(TriggerMode_Off);
        }
      }

      // Ajusta o modo de aquisição para single frame TODO: ver se continuous é melhor
      camera->AcquisitionMode.SetValue(AcquisitionMode_SingleFrame);

      // Ajusta o modo de exibição TODO: ajustar o melhor parâmetro, e ver se dá pra fazer isso durante a aquisição
      camera->ExposureMode.SetValue(ExposureMode_Timed);
      camera->ExposureTimeRaw.SetValue(9000);

      // Create an image buffer
      size_t image_size = (size_t)(camera->PayloadSize.GetValue());
      pbuffer = new uint8_t[image_size];

      // We won't use image buffers greater than ImageSize
      stream_grabber->MaxBufferSize.SetValue(image_size);

      // We won't queue more than one image buffer at a time
      stream_grabber->MaxNumBuffer.SetValue(1);

      // Allocate all resources for grabbing. Critical parameters like image
      // size now must not be changed until FinishGrab() is called.
      stream_grabber->PrepareGrab();

      // Buffers used for grabbing must be registered at the stream grabber.
      // The registration returns a handle to be used for queuing the buffer.
      hbuffer = stream_grabber->RegisterBuffer(pbuffer, image_size);

      // Put the buffer into the grab queue for grabbing
      stream_grabber->QueueBuffer(hbuffer, NULL);

      initialized = true;
    } catch (GenICam::GenericException &e) {
      // Error handling
      error_message = e.GetDescription();
    }
  }
}

void CameraAbstractor::Finalize() {
  error_message = "";

  if (initialized) {
    if (stream_grabber != NULL) {
      if (hbuffer != NULL) {
        // You must deregister the buffers before freeing the memory
        stream_grabber->DeregisterBuffer(hbuffer);
      }

      // Free all resources used for grabbing
      stream_grabber->FinishGrab();

      // Close stream grabber
      stream_grabber->Close();
    }

    if (camera != NULL) {
      // Close camera
      camera->Close();
    }

    if (pbuffer != NULL) {
      // Free memory of image buffer
      delete[] pbuffer;
    }
  }
}

bool CameraAbstractor::GetImage(uint32_t** area_to_put_image) {
  error_message = "";
  bool worked = false;

  try {
    // Frame por frame... TODO: talvez isso não seja o mais eficiente!
    camera->AcquisitionStart.Execute();

    // Wait for the grabbed image with a timeout of 3 seconds
    if (stream_grabber->GetWaitObject().Wait(5000)) {
      // Get the grab result from the grabber's result queue
      GrabResult result;
      stream_grabber->RetrieveResult(result);

      if (result.Succeeded()) {
        // Get the pointer to the image buffer
        const uint8_t *p_image_buffer = (uint8_t *) result.Buffer();

        for (int i = 0; i < image_height; ++i) {
          for (int j = 0; j < image_width; ++j) {
            area_to_put_image[i][j] = (uint32_t) p_image_buffer[i*image_width + j]; //TODO: verificar se a convenção é essa mesma
          }
        }

        stream_grabber->QueueBuffer(result.Handle(), NULL);

        worked = true;
      } else {
        error_message = result.GetErrorDescription();
      }
    } else {
      error_message = "Timeout occurred!";

      // Get the pending buffer back (You are not allowed to deregister
      // buffers when they are still queued)
      stream_grabber->CancelGrab();

      // Get all buffers back
      for (GrabResult r; stream_grabber->RetrieveResult(r););
    }
  } catch (GenICam::GenericException &e) {
    error_message = e.GetDescription();
  }

  return worked;
}

string CameraAbstractor::GetErrorMessage() {
  return error_message;
}

bool CameraAbstractor::IsInitialized() {
  return initialized;
}

int64_t CameraAbstractor::GetConfiguredImagesWidth() {
  return this->image_width;
}

int64_t CameraAbstractor::GetConfiguredImagesHeight() {
  return this->image_height;
}

